{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 02 无驱动的仿真\n",
    "\n",
    "在导入机器人的同时加入地面，并在无驱动的情况下运行一段时间的仿真。\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 重复加载机器人的过程"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import importlib\n",
    "import sys\n",
    "from urllib.request import urlretrieve"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "server_args = []\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Imports\n",
    "import numpy as np\n",
    "import pydot\n",
    "from ipywidgets import Dropdown, Layout\n",
    "from IPython.display import display, HTML, SVG\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
    "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
    "    MultibodyPlant, Parser, Simulator,\n",
    "    # add for ground model\n",
    "    RigidTransform, CoulombFriction, HalfSpace,\n",
    "    # add for single trunk model\n",
    "    #GeometryFrame, Box, Sphere, GeometryInstance, MakePhongIllustrationProperties\n",
    ")\n",
    "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "这里加载机器人并加入地面"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6003...\nYou can open the visualizer by visiting the following URL:\nhttp://127.0.0.1:7003/static/\nConnected to meshcat-server.\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_fl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "53b4d87a15c84ed59278a9fda3b71f03"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_fl_to_thigh_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "c526406efc824a7bb4049f53e37521f6"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_fl_to_knee_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "4846d04bd2ea45b28cb5ba95304ae8ff"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_fr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "a7ae69e6d4b945c2a36d4649dfa36af7"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_fr_to_thigh_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "983c43d051374ec7bc767c17eabe8678"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_fr_to_knee_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "72b227b501e24b96810f668b5b784abc"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_hl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "668df28863f148b3a6fb69f753bc16ea"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_hl_to_thigh_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "61713c9886b94d2fa66315fe66113c97"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_hl_to_knee_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "8aec22ee405e4e4397620242d8ee454d"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_hr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "2403114f518e4af2a5bd6bc767a94619"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_hr_to_thigh_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "d5fd1d0da4f744f8a80ceb16f5459b9a"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_hr_to_knee_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "3aec92787ffb4f55970c10b87f4ba535"
      }
     },
     "metadata": {}
    }
   ],
   "source": [
    "builder = DiagramBuilder()\n",
    "\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=5e-3)\n",
    "\n",
    "#Parser(plant, scene_graph).AddModelFromFile(\"../models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "\n",
    "# Add a flat ground with friction\n",
    "X_BG = RigidTransform()\n",
    "surface_friction = CoulombFriction(\n",
    "        static_friction = 1.0,\n",
    "        dynamic_friction = 1.0)\n",
    "plant.RegisterCollisionGeometry(\n",
    "        plant.world_body(),      # the body for which this object is registered\n",
    "        X_BG,                    # The fixed pose of the geometry frame G in the body frame B\n",
    "        HalfSpace(),             # Defines the geometry of the object\n",
    "        \"ground_collision\",      # A name\n",
    "        surface_friction)        # Coulomb friction coefficients\n",
    "plant.RegisterVisualGeometry(\n",
    "        plant.world_body(),\n",
    "        X_BG,\n",
    "        HalfSpace(),\n",
    "        \"ground_visual\",\n",
    "        np.array([0.5,0.5,0.5,0.0]))    # Color set to be completely transparent\n",
    "\n",
    "plant.Finalize()\n",
    "assert plant.geometry_source_is_registered()\n",
    "\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
    "diagram = builder.Build()\n",
    "diagram_context = diagram.CreateDefaultContext()\n",
    "\n",
    "meshcat.load()\n",
    "MakeJointSlidersThatPublishOnCallback(plant, meshcat, diagram_context);\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 设置初始状态，并将所有输入固定为0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7fd20f7cc070>"
      ]
     },
     "metadata": {},
     "execution_count": 6
    }
   ],
   "source": [
    "use_lcm = False\n",
    "\n",
    "target_realtime_rate = 1.0\n",
    "\n",
    "# Simulator setup\n",
    "simulator = Simulator(diagram, diagram_context)\n",
    "if use_lcm:\n",
    "    # If we're using LCM to send messages to another simulator or a real\n",
    "    # robot, we don't want Drake to slow things down, so we'll publish\n",
    "    # new messages as fast as possible\n",
    "    simulator.set_target_realtime_rate(0.0)\n",
    "else:\n",
    "    simulator.set_target_realtime_rate(target_realtime_rate)\n",
    "    \n",
    "# Set initial states\n",
    "plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)\n",
    "\n",
    "q0 = np.asarray([ 1.0, 0.0, 0.0, 0.0,     # base orientation\n",
    "                  0.0, 0.0, 0.3,          # base position\n",
    "                  0.0, 0.0, 0.0, 0.0,     # ad/ab\n",
    "                 -0.8,-0.8,-0.8,-0.8,     # hip\n",
    "                  1.6, 1.6, 1.6, 1.6])    # knee\n",
    "qd0 = np.zeros(plant.num_velocities())\n",
    "plant.SetPositions(plant_context,q0)\n",
    "plant.SetVelocities(plant_context,qd0)\n",
    "\n",
    "## another way for fixing the input port to zero\n",
    "# Fix the input port to zero.\n",
    "#    plant_context = diagram.GetMutableSubsystemContext(\n",
    "#        plant, simulator.get_mutable_context())\n",
    "plant.get_actuation_input_port().FixValue(\n",
    "        plant_context, np.zeros(plant.num_actuators()))\n",
    "\n",
    "# Run the simulation a bit\n",
    "simulator.AdvanceTo(0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f1ba922c4f0>"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#sim_time = 6.0\n",
    "sim_time = 0.5\n",
    "\n",
    "# Run the simulation!\n",
    "simulator.AdvanceTo(sim_time)"
   ]
  },
  {
   "source": [
    "## 现象\n",
    "\n",
    "平台建立正常。\n",
    "仿真过程正常。\n",
    "\n",
    "需要注意的是，最开始仿真先运行到0.1秒是因为直接运行一段时间时，前面的动画过程看不到。"
   ],
   "cell_type": "markdown",
   "metadata": {}
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7-final"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}